#include <iostream>
#include <gps_transform/navsat_conversions.hpp>
#include <gps_transform/navsat_transform.hpp>
#include <sensor_msgs/msg/nav_sat_fix.hpp>
#include <nav_msgs/msg/odometry.hpp>
#include <GeographicLib/Geocentric.hpp>
#include <GeographicLib/LocalCartesian.hpp>
const int POSITION_SIZE = 3;
const int POSE_SIZE = 6;
using std::placeholders::_1;
NavsatTransformer::NavsatTransformer(const rclcpp::NodeOptions &options)
    : rclcpp::Node("navsat_transformer", options),
      // 初始化代码
      gps_frame_id_(""),
      gps_updated_(false),
      has_transform_gps_(false),
      // has_transform_imu_(false),
      // has_transform_odom_(false),
      // magnetic_declination_(0.0),
      // odom_updated_(false),
      // publish_gps_(false),
      transform_good_(false),
      // transform_timeout_(0ns),
      use_local_cartesian_(false),
      use_manual_datum_(false),
      // use_odometry_yaw_(false),
      // cartesian_broadcaster_(*this),
      utm_meridian_convergence_(0.0),
      utm_zone_(""),
      has_read_imu_ori(false)
      // world_frame_id_("odom"),
      // yaw_offset_(0.0),
//       zero_altitude_(false)
{
	  latest_cartesian_covariance_.resize(POSE_SIZE, POSE_SIZE);
    auto custom_qos = rclcpp::SensorDataQoS(rclcpp::KeepLast(1));
    utm_odom_pub_ = this->create_publisher<nav_msgs::msg::Odometry>("gps_utm", 10);
    gps_sub_ = this->create_subscription<sensor_msgs::msg::NavSatFix>(
    "fix", custom_qos, std::bind(&NavsatTransformer::gpsFixCallback, this, _1));
    imu_sub_ = this->create_subscription<sensor_msgs::msg::Imu>(
      // "imu", custom_qos, std::bind(&NavSatTransform::imuCallback, this, _1));
      "imu_orientation", custom_qos, std::bind(&NavsatTransformer::imuCallback, this, _1));
}
NavsatTransformer::~NavsatTransformer()
{}

void NavsatTransformer::gpsFixCallback(
  const sensor_msgs::msg::NavSatFix::SharedPtr msg)
{
  gps_frame_id_ = msg->header.frame_id;

  if (gps_frame_id_.empty()) {
    RCLCPP_ERROR(
      this->get_logger(),
      "NavSatFix message has empty frame_id. "
      "Will assume navsat device is mounted at robot's origin");
  }

  // Make sure the GPS data is usable
  bool good_gps =
    (msg->status.status != sensor_msgs::msg::NavSatStatus::STATUS_NO_FIX &&
   // msg->status.status != sensor_msgs::msg::NavSatStatus::STATUS_FIX &&
    !std::isnan(msg->altitude) && !std::isnan(msg->latitude) &&
    !std::isnan(msg->longitude));

  if (good_gps) {
    // If we haven't computed the transform yet, then
    // store this message as the initial GPS data to use
    if (!transform_good_ && !use_manual_datum_) {
      setTransformGps(msg);
    }

    double cartesian_x = 0;
    double cartesian_y = 0;
    std::string cartesian_zone_tmp;
    navsat_conversions::LLtoUTM(
      msg->latitude,
      msg->longitude,
      cartesian_y,
      cartesian_x,
      cartesian_zone_tmp);
    latest_cartesian_pose_.setOrigin(tf2::Vector3(cartesian_x, cartesian_y, msg->altitude));
    latest_cartesian_covariance_.setZero();
    if (has_read_imu_ori){
	    // RCLCPP_INFO(this->get_logger(), "pub utm (%f, %f, %f, %f)",gps_orientation.x,gps_orientation.y,gps_orientation.z,gps_orientation.w);
      nav_msgs::msg::Odometry utm_odom;
      utm_odom.pose.pose.orientation.x = gps_orientation.x;
      utm_odom.pose.pose.orientation.y = gps_orientation.y;
      utm_odom.pose.pose.orientation.z = gps_orientation.z;
      utm_odom.pose.pose.orientation.w = gps_orientation.w;
      utm_odom.pose.pose.position.x = cartesian_x;
      utm_odom.pose.pose.position.y = cartesian_y;
      utm_odom.pose.pose.position.z = msg->altitude;
      utm_odom.header.frame_id = "UTM"; //NOT the world(ROBOT) frame!!!
      utm_odom.child_frame_id = "RTK";
      utm_odom.header.stamp = msg->header.stamp;
      utm_odom_pub_->publish(utm_odom);
      RCLCPP_INFO(this->get_logger(), "pub utm finished");

      // Copy the measurement's covariance matrix so that we can rotate it later
      for (size_t i = 0; i < POSITION_SIZE; i++) {
        for (size_t j = 0; j < POSITION_SIZE; j++) {
          latest_cartesian_covariance_(i, j) =
            msg->position_covariance[POSITION_SIZE * i + j];
        }
      }
      gps_update_time_ = msg->header.stamp;
      gps_updated_ = true;
      has_read_imu_ori = false;
    }
  }
}


void NavsatTransformer::setTransformGps(
  const sensor_msgs::msg::NavSatFix::SharedPtr & msg)
{
  // wangshuo, for test
  // std::cout << __FILE__ << ":" << __LINE__ << std::endl;
  // std::cout << "LLA:" << msg->latitude << " " << msg->longitude << " " << msg->altitude << std::endl;
  double cartesian_x {};
  double cartesian_y {};
  double cartesian_z {};
  if (use_local_cartesian_) {
    const double hae_altitude {};
    gps_local_cartesian_.Reset(msg->latitude, msg->longitude, hae_altitude);
    gps_local_cartesian_.Forward(
      msg->latitude,
      msg->longitude,
      msg->altitude,
      cartesian_x,
      cartesian_y,
      cartesian_z);

    // UTM meridian convergence is not meaningful when using local cartesian, so set it to 0.0
    utm_meridian_convergence_ = 0.0;
  } else {
    navsat_conversions::LLtoUTM(
      msg->latitude,
      msg->longitude,
      cartesian_y,
      cartesian_x,
      utm_zone_,
      utm_meridian_convergence_);
    utm_meridian_convergence_ *= navsat_conversions::RADIANS_PER_DEGREE;
  }

  RCLCPP_INFO(
    this->get_logger(), "Datum (latitude, longitude, altitude) is (%0.2f, %0.2f, %0.2f)",
    msg->latitude, msg->longitude, msg->altitude);
  RCLCPP_INFO(
    this->get_logger(), "Datum %s coordinate is (%s, %0.2f, %0.2f)",
    ((use_local_cartesian_) ? "Local Cartesian" : "UTM"), utm_zone_.c_str(), cartesian_x,
    cartesian_y);

  transform_cartesian_pose_.setOrigin(tf2::Vector3(cartesian_x, cartesian_y, msg->altitude));
  transform_cartesian_pose_.setRotation(tf2::Quaternion::getIdentity());


  transform_cartesian_pose_timestamp_ = msg->header.stamp.sec 
                                      + msg->header.stamp.nanosec / 1000000000.0;

  has_transform_gps_ = true;
}

void NavsatTransformer::imuCallback(const sensor_msgs::msg::Imu::SharedPtr msg)
{
  gps_orientation = msg->orientation;
  RCLCPP_INFO(this->get_logger(), "read imu_orientation");
  has_read_imu_ori = true;
}

int main(int argc, char ** argv)
{
  rclcpp::init(argc, argv);

  const rclcpp::NodeOptions options;
  auto navsat_transform_node = std::make_shared<NavsatTransformer>(options);
  rclcpp::spin(navsat_transform_node->get_node_base_interface());
  rclcpp::shutdown();
  return 0;
}
